#include "buff_detector.hpp"
#include "camera.hpp"
#include "compensator.hpp"
#include "opencv2/opencv.hpp"
#include "robot.hpp"
#include "spdlog/sinks/basic_file_sink.h"
#include "spdlog/sinks/stdout_color_sinks.h"
#include "spdlog/spdlog.h"

int main(int argc, char const *argv[]) {
  (void)argc;
  (void)argv;

  auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
  auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>(
      "/home/dji/Desktop/logs/buff.log", true);

  spdlog::sinks_init_list sink_list = {console_sink, file_sink};

  spdlog::set_default_logger(
      std::make_shared<spdlog::logger>("default", sink_list));

#if (SPDLOG_ACTIVE_LEVEL == SPDLOG_LEVEL_DEBUG)
  spdlog::flush_on(spdlog::level::debug);
  spdlog::set_level(spdlog::level::debug);
#elif (SPDLOG_ACTIVE_LEVEL == SPDLOG_LEVEL_INFO)
  spdlog::flush_on(spdlog::level::info);
  spdlog::set_level(spdlog::level::info);
#endif

  SPDLOG_WARN("***** Running buff. *****");

  Robot robot("/dev/ttyTHS2");
  Compensator compensator("../../../../runtime/MV-CA016-10UC-6mm.json");
  Camera cam(0, 640, 480);
  cv::Mat frame;

  do {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    frame = cam.GetFrame();  //调用以清除相机缓存
  } while (robot.GetTeam() == game::Team::kUNKNOWN);  // note

  const game::Team team = game::Team::kBLUE;//--------------------------note: kRED 为识别红色装甲板
  BuffDetector buff_detector("../../../../runtime/RMUT2021_Buff.json", team);

  do {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    frame = cam.GetFrame();      //调用以清除相机缓存
  } while (robot.StartGame());  //待机等待比赛开始----------------------note:已添加,调试请while(!robot.StartGame())
  buff_detector.Restart();       //比赛开始,记录当前时间

  SPDLOG_WARN("Start the game!!!");
  while (true) {
    frame = cam.GetFrame();
    if (frame.empty()) continue;
    auto armors = buff_detector.Detect(frame);

    if (armors.size() > 0) {
      SPDLOG_WARN("armors's size is {}", armors.size());
      compensator.Apply(armors, frame, robot.GetBalletSpeed(), robot.GetEuler().pitch);
      robot.Aim(armors.front().GetAimEuler(), false);
    }
    //buff_detector.VisualizeResult(frame, 3);
    //cv::imshow("window", frame);
    //cv::waitKey(1);
  }

  return EXIT_SUCCESS;
}
